/*****************************************************************//**
 * \file   EndToolForcep.h
 * \brief  关于Forcep相关操作
 *
 *
 * \author Administrator
 * \date   July 2021
 * \modify by Administrator
 * \date   July 2021

 *********************************************************************/
#pragma once

#include "Eigen/Dense"
#include "../EndToolInterface.h"

#define PI (3.1415926535897932346f)

class EndToolForcep :public EndToolInterface
{
public:
	bool bInitDone = false;
public:
	//手术器械长度、手术器械在体外的长度，当前位置与目标位置的差值；
	double instrument_length = 0.35;
	double instrument_out, distance_oldAndnew;
	bool Isloutset;

	//机械臂末端到手术器械末端的三次转换矩阵
	Eigen::Matrix4d transM1, transM2, transM3;

	//机械臂末端矩阵To手术器械末端的转换矩阵
	Eigen::Matrix4d transMatrix_1, transMatrix;

public:
	EndToolForcep();
	~EndToolForcep();

	//获取手术器械末端的位姿
	Eigen::Matrix4d geToolEndMatrix(Eigen::Matrix4d robotend) override;

	//通过RCM创口点约束，将主手位姿加持到器械末端后，求得机械臂末端
	Eigen::Matrix4d getRobotNextEnd(Eigen::Matrix4d robotend_current, Eigen::Matrix4d toolend_next, Eigen::Vector3d RCM) override;
	//获取机械臂末端手术器械种类
	int getEndToolType() override;
};
